<!-- HTML header for doxygen 1.8.6-->
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.13"/>
<title>OpenCV: samples/cpp/kalman.cpp</title>
<link href="../../opencv.ico" rel="shortcut icon" type="image/x-icon" />
<link href="../../tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../jquery.js"></script>
<script type="text/javascript" src="../../dynsections.js"></script>
<script type="text/javascript" src="../../tutorial-utils.js"></script>
<link href="../../search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../search/searchdata.js"></script>
<script type="text/javascript" src="../../search/search.js"></script>
<script type="text/x-mathjax-config">
  MathJax.Hub.Config({
    extensions: ["tex2jax.js", "TeX/AMSmath.js", "TeX/AMSsymbols.js"],
    jax: ["input/TeX","output/HTML-CSS"],
});
//<![CDATA[
MathJax.Hub.Config(
{
  TeX: {
      Macros: {
          matTT: [ "\\[ \\left|\\begin{array}{ccc} #1 & #2 & #3\\\\ #4 & #5 & #6\\\\ #7 & #8 & #9 \\end{array}\\right| \\]", 9],
          fork: ["\\left\\{ \\begin{array}{l l} #1 & \\mbox{#2}\\\\ #3 & \\mbox{#4}\\\\ \\end{array} \\right.", 4],
          forkthree: ["\\left\\{ \\begin{array}{l l} #1 & \\mbox{#2}\\\\ #3 & \\mbox{#4}\\\\ #5 & \\mbox{#6}\\\\ \\end{array} \\right.", 6],
          forkfour: ["\\left\\{ \\begin{array}{l l} #1 & \\mbox{#2}\\\\ #3 & \\mbox{#4}\\\\ #5 & \\mbox{#6}\\\\ #7 & \\mbox{#8}\\\\ \\end{array} \\right.", 8],
          vecthree: ["\\begin{bmatrix} #1\\\\ #2\\\\ #3 \\end{bmatrix}", 3],
          vecthreethree: ["\\begin{bmatrix} #1 & #2 & #3\\\\ #4 & #5 & #6\\\\ #7 & #8 & #9 \\end{bmatrix}", 9],
          cameramatrix: ["#1 = \\begin{bmatrix} f_x & 0 & c_x\\\\ 0 & f_y & c_y\\\\ 0 & 0 & 1 \\end{bmatrix}", 1],
          distcoeffs: ["(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \\tau_x, \\tau_y]]]]) \\text{ of 4, 5, 8, 12 or 14 elements}"],
          distcoeffsfisheye: ["(k_1, k_2, k_3, k_4)"],
          hdotsfor: ["\\dots", 1],
          mathbbm: ["\\mathbb{#1}", 1],
          bordermatrix: ["\\matrix{#1}", 1]
      }
  }
}
);
//]]>
</script><script type="text/javascript" src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.0/MathJax.js"></script>
<link href="../../doxygen.css" rel="stylesheet" type="text/css" />
<link href="../../stylesheet.css" rel="stylesheet" type="text/css"/>
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<!--#include virtual="/google-search.html"-->
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectlogo"><img alt="Logo" src="../../opencv-logo-small.png"/></td>
  <td style="padding-left: 0.5em;">
   <div id="projectname">OpenCV
   &#160;<span id="projectnumber">4.5.2</span>
   </div>
   <div id="projectbrief">Open Source Computer Vision</div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.13 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "../../search",false,'Search');
</script>
<script type="text/javascript" src="../../menudata.js"></script>
<script type="text/javascript" src="../../menu.js"></script>
<script type="text/javascript">
$(function() {
  initMenu('../../',true,false,'search.php','Search');
  $(document).ready(function() { init_search(); });
});
</script>
<div id="main-nav"></div>
</div><!-- top -->
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div class="header">
  <div class="headertitle">
<div class="title">samples/cpp/kalman.cpp</div>  </div>
</div><!--header-->
<div class="contents">
<p>An example using the standard Kalman filter</p>
<div class="fragment"><div class="line"><span class="preprocessor">#include &quot;<a class="code" href="../../d4/dc2/modules_2video_2include_2opencv2_2video_2tracking_8hpp.html">opencv2/video/tracking.hpp</a>&quot;</span></div><div class="line"><span class="preprocessor">#include &quot;<a class="code" href="../../d4/dd5/highgui_8hpp.html">opencv2/highgui.hpp</a>&quot;</span></div><div class="line"></div><div class="line"><span class="preprocessor">#include &lt;stdio.h&gt;</span></div><div class="line"></div><div class="line"><span class="keyword">using namespace </span><a class="code" href="../../d2/d75/namespacecv.html">cv</a>;</div><div class="line"></div><div class="line"><span class="keyword">static</span> <span class="keyword">inline</span> <a name="_a0"></a><a class="code" href="../../db/d4e/classcv_1_1Point__.html">Point</a> calcPoint(<a name="_a1"></a><a class="code" href="../../db/d4e/classcv_1_1Point__.html">Point2f</a> center, <span class="keywordtype">double</span> R, <span class="keywordtype">double</span> angle)</div><div class="line">{</div><div class="line">    <span class="keywordflow">return</span> center + <a name="a2"></a><a class="code" href="../../dc/d84/group__core__basic.html#ga7d080aa40de011e4410bca63385ffe2a">Point2f</a>((<span class="keywordtype">float</span>)<a name="a3"></a><a class="code" href="../../d0/de1/group__core.html#gaf0f2fe47183d063fb7415097fbadb570">cos</a>(angle), (<span class="keywordtype">float</span>)-<a name="a4"></a><a class="code" href="../../d0/de1/group__core.html#ga53a8656033a51db64caa72ee9d4e93b4">sin</a>(angle))*(float)R;</div><div class="line">}</div><div class="line"></div><div class="line"><span class="keyword">static</span> <span class="keywordtype">void</span> help()</div><div class="line">{</div><div class="line">    printf( <span class="stringliteral">&quot;\nExample of c calls to OpenCV&#39;s Kalman filter.\n&quot;</span></div><div class="line"><span class="stringliteral">&quot;   Tracking of rotating point.\n&quot;</span></div><div class="line"><span class="stringliteral">&quot;   Rotation speed is constant.\n&quot;</span></div><div class="line"><span class="stringliteral">&quot;   Both state and measurements vectors are 1D (a point angle),\n&quot;</span></div><div class="line"><span class="stringliteral">&quot;   Measurement is the real point angle + gaussian noise.\n&quot;</span></div><div class="line"><span class="stringliteral">&quot;   The real and the estimated points are connected with yellow line segment,\n&quot;</span></div><div class="line"><span class="stringliteral">&quot;   the real and the measured points are connected with red line segment.\n&quot;</span></div><div class="line"><span class="stringliteral">&quot;   (if Kalman filter works correctly,\n&quot;</span></div><div class="line"><span class="stringliteral">&quot;    the yellow segment should be shorter than the red one).\n&quot;</span></div><div class="line">            <span class="stringliteral">&quot;\n&quot;</span></div><div class="line"><span class="stringliteral">&quot;   Pressing any key (except ESC) will reset the tracking with a different speed.\n&quot;</span></div><div class="line"><span class="stringliteral">&quot;   Pressing ESC will stop the program.\n&quot;</span></div><div class="line">            );</div><div class="line">}</div><div class="line"></div><div class="line"><span class="keywordtype">int</span> main(<span class="keywordtype">int</span>, <span class="keywordtype">char</span>**)</div><div class="line">{</div><div class="line">    help();</div><div class="line">    <a name="_a5"></a><a class="code" href="../../d3/d63/classcv_1_1Mat.html">Mat</a> img(500, 500, <a name="a6"></a><a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga88c4cd9de76f678f33928ef1e3f96047">CV_8UC3</a>);</div><div class="line">    <a name="_a7"></a><a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html">KalmanFilter</a> KF(2, 1, 0);</div><div class="line">    <a class="code" href="../../d3/d63/classcv_1_1Mat.html">Mat</a> state(2, 1, <a name="a8"></a><a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga4a3def5d72b74bed31f5f8ab7676099c">CV_32F</a>); <span class="comment">/* (phi, delta_phi) */</span></div><div class="line">    <a class="code" href="../../d3/d63/classcv_1_1Mat.html">Mat</a> processNoise(2, 1, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga4a3def5d72b74bed31f5f8ab7676099c">CV_32F</a>);</div><div class="line">    <a class="code" href="../../d3/d63/classcv_1_1Mat.html">Mat</a> measurement = <a name="a9"></a><a class="code" href="../../d3/d63/classcv_1_1Mat.html#a0b57b6a326c8876d944d188a46e0f556">Mat::zeros</a>(1, 1, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga4a3def5d72b74bed31f5f8ab7676099c">CV_32F</a>);</div><div class="line">    <span class="keywordtype">char</span> code = (char)-1;</div><div class="line"></div><div class="line">    <span class="keywordflow">for</span>(;;)</div><div class="line">    {</div><div class="line">        <a name="a10"></a><a class="code" href="../../d2/de8/group__core__array.html#gaeff1f61e972d133a04ce3a5f81cf6808">randn</a>( state, <a name="a11"></a><a class="code" href="../../d1/da0/classcv_1_1Scalar__.html#ac1509a4b8454fe7fe29db069e13a2e6f">Scalar::all</a>(0), <a class="code" href="../../d1/da0/classcv_1_1Scalar__.html#ac1509a4b8454fe7fe29db069e13a2e6f">Scalar::all</a>(0.1) );</div><div class="line">        KF.<a name="a12"></a><a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a> = (<a name="_a13"></a><a class="code" href="../../df/dfc/classcv_1_1Mat__.html">Mat_&lt;float&gt;</a>(2, 2) &lt;&lt; 1, 1, 0, 1);</div><div class="line"></div><div class="line">        <a name="a14"></a><a class="code" href="../../d2/de8/group__core__array.html#ga388d7575224a4a277ceb98ccaa327c99">setIdentity</a>(KF.<a name="a15"></a><a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0f60b78726d8eccf74a1f2479c2d1f97">measurementMatrix</a>);</div><div class="line">        <a class="code" href="../../d2/de8/group__core__array.html#ga388d7575224a4a277ceb98ccaa327c99">setIdentity</a>(KF.<a name="a16"></a><a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#af19be9c0630d0f658bdbaea409a35cda">processNoiseCov</a>, <a class="code" href="../../d1/da0/classcv_1_1Scalar__.html#ac1509a4b8454fe7fe29db069e13a2e6f">Scalar::all</a>(1e-5));</div><div class="line">        <a class="code" href="../../d2/de8/group__core__array.html#ga388d7575224a4a277ceb98ccaa327c99">setIdentity</a>(KF.<a name="a17"></a><a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a828d051035ba807966ad65edf288a08e">measurementNoiseCov</a>, <a class="code" href="../../d1/da0/classcv_1_1Scalar__.html#ac1509a4b8454fe7fe29db069e13a2e6f">Scalar::all</a>(1e-1));</div><div class="line">        <a class="code" href="../../d2/de8/group__core__array.html#ga388d7575224a4a277ceb98ccaa327c99">setIdentity</a>(KF.<a name="a18"></a><a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a446d8e9a0105b0aa35cd66119c529803">errorCovPost</a>, <a class="code" href="../../d1/da0/classcv_1_1Scalar__.html#ac1509a4b8454fe7fe29db069e13a2e6f">Scalar::all</a>(1));</div><div class="line"></div><div class="line">        <a class="code" href="../../d2/de8/group__core__array.html#gaeff1f61e972d133a04ce3a5f81cf6808">randn</a>(KF.<a name="a19"></a><a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#add8fb5ac9c04b4600b679698dcb0447d">statePost</a>, <a class="code" href="../../d1/da0/classcv_1_1Scalar__.html#ac1509a4b8454fe7fe29db069e13a2e6f">Scalar::all</a>(0), <a class="code" href="../../d1/da0/classcv_1_1Scalar__.html#ac1509a4b8454fe7fe29db069e13a2e6f">Scalar::all</a>(0.1));</div><div class="line"></div><div class="line">        <span class="keywordflow">for</span>(;;)</div><div class="line">        {</div><div class="line">            <a class="code" href="../../db/d4e/classcv_1_1Point__.html">Point2f</a> center(img.<a name="a20"></a><a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa3e5a47585c9ef6a0842556739155e3e">cols</a>*0.5f, img.<a name="a21"></a><a class="code" href="../../d3/d63/classcv_1_1Mat.html#abed816466c45234254d25bc59c31245e">rows</a>*0.5f);</div><div class="line">            <span class="keywordtype">float</span> R = img.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa3e5a47585c9ef6a0842556739155e3e">cols</a>/3.f;</div><div class="line">            <span class="keywordtype">double</span> stateAngle = state.<a name="a22"></a><a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">float</span>&gt;(0);</div><div class="line">            <a class="code" href="../../db/d4e/classcv_1_1Point__.html">Point</a> statePt = calcPoint(center, R, stateAngle);</div><div class="line"></div><div class="line">            <a class="code" href="../../d3/d63/classcv_1_1Mat.html">Mat</a> prediction = KF.<a name="a23"></a><a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#aa710d2255566bec8d6ce608d103d4fa7">predict</a>();</div><div class="line">            <span class="keywordtype">double</span> predictAngle = prediction.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">float</span>&gt;(0);</div><div class="line">            <a class="code" href="../../db/d4e/classcv_1_1Point__.html">Point</a> predictPt = calcPoint(center, R, predictAngle);</div><div class="line"></div><div class="line">            <a class="code" href="../../d2/de8/group__core__array.html#gaeff1f61e972d133a04ce3a5f81cf6808">randn</a>( measurement, <a class="code" href="../../d1/da0/classcv_1_1Scalar__.html#ac1509a4b8454fe7fe29db069e13a2e6f">Scalar::all</a>(0), <a class="code" href="../../d1/da0/classcv_1_1Scalar__.html#ac1509a4b8454fe7fe29db069e13a2e6f">Scalar::all</a>(KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a828d051035ba807966ad65edf288a08e">measurementNoiseCov</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">float</span>&gt;(0)));</div><div class="line"></div><div class="line">            <span class="comment">// generate measurement</span></div><div class="line">            measurement += KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0f60b78726d8eccf74a1f2479c2d1f97">measurementMatrix</a>*state;</div><div class="line"></div><div class="line">            <span class="keywordtype">double</span> measAngle = measurement.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">float</span>&gt;(0);</div><div class="line">            <a class="code" href="../../db/d4e/classcv_1_1Point__.html">Point</a> measPt = calcPoint(center, R, measAngle);</div><div class="line"></div><div class="line">            <span class="comment">// plot points</span></div><div class="line"><span class="preprocessor">            #define drawCross( center, color, d )                                        \</span></div><div class="line"><span class="preprocessor">                line( img, Point( center.x - d, center.y - d ),                          \</span></div><div class="line"><span class="preprocessor">                             Point( center.x + d, center.y + d ), color, 1, LINE_AA, 0); \</span></div><div class="line"><span class="preprocessor">                line( img, Point( center.x + d, center.y - d ),                          \</span></div><div class="line"><span class="preprocessor">                             Point( center.x - d, center.y + d ), color, 1, LINE_AA, 0 )</span></div><div class="line"></div><div class="line">            img = <a class="code" href="../../d1/da0/classcv_1_1Scalar__.html#ac1509a4b8454fe7fe29db069e13a2e6f">Scalar::all</a>(0);</div><div class="line">            drawCross( statePt, <a name="a24"></a><a class="code" href="../../dc/d84/group__core__basic.html#ga599fe92e910c027be274233eccad7beb">Scalar</a>(255,255,255), 3 );</div><div class="line">            drawCross( measPt, <a class="code" href="../../dc/d84/group__core__basic.html#ga599fe92e910c027be274233eccad7beb">Scalar</a>(0,0,255), 3 );</div><div class="line">            drawCross( predictPt, <a class="code" href="../../dc/d84/group__core__basic.html#ga599fe92e910c027be274233eccad7beb">Scalar</a>(0,255,0), 3 );</div><div class="line">            <a name="a25"></a><a class="code" href="../../d6/d6e/group__imgproc__draw.html#ga7078a9fae8c7e7d13d24dac2520ae4a2">line</a>( img, statePt, measPt, <a class="code" href="../../dc/d84/group__core__basic.html#ga599fe92e910c027be274233eccad7beb">Scalar</a>(0,0,255), 3, <a name="a26"></a><a class="code" href="../../d6/d6e/group__imgproc__draw.html#ggaf076ef45de481ac96e0ab3dc2c29a777a85fdabe5335c9e6656563dfd7c94fb4f">LINE_AA</a>, 0 );</div><div class="line">            <a class="code" href="../../d6/d6e/group__imgproc__draw.html#ga7078a9fae8c7e7d13d24dac2520ae4a2">line</a>( img, statePt, predictPt, <a class="code" href="../../dc/d84/group__core__basic.html#ga599fe92e910c027be274233eccad7beb">Scalar</a>(0,255,255), 3, <a class="code" href="../../d6/d6e/group__imgproc__draw.html#ggaf076ef45de481ac96e0ab3dc2c29a777a85fdabe5335c9e6656563dfd7c94fb4f">LINE_AA</a>, 0 );</div><div class="line"></div><div class="line">            <span class="keywordflow">if</span>(<a name="a27"></a><a class="code" href="../../d2/de8/group__core__array.html#ga75843061d150ad6564b5447e38e57722">theRNG</a>().uniform(0,4) != 0)</div><div class="line">                KF.<a name="a28"></a><a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a60eb7feb569222ad0657ef1875884b5e">correct</a>(measurement);</div><div class="line"></div><div class="line">            <a class="code" href="../../d2/de8/group__core__array.html#gaeff1f61e972d133a04ce3a5f81cf6808">randn</a>( processNoise, <a class="code" href="../../dc/d84/group__core__basic.html#ga599fe92e910c027be274233eccad7beb">Scalar</a>(0), <a class="code" href="../../d1/da0/classcv_1_1Scalar__.html#ac1509a4b8454fe7fe29db069e13a2e6f">Scalar::all</a>(<a name="a29"></a><a class="code" href="../../d0/de1/group__core.html#ga9070b6a3f093dd952d973819b06f4906">sqrt</a>(KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#af19be9c0630d0f658bdbaea409a35cda">processNoiseCov</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">float</span>&gt;(0, 0))));</div><div class="line">            state = KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>*state + processNoise;</div><div class="line"></div><div class="line">            <a name="a30"></a><a class="code" href="../../d7/dfc/group__highgui.html#ga453d42fe4cb60e5723281a89973ee563">imshow</a>( <span class="stringliteral">&quot;Kalman&quot;</span>, img );</div><div class="line">            code = (char)<a name="a31"></a><a class="code" href="../../d7/dfc/group__highgui.html#ga5628525ad33f52eab17feebcfba38bd7">waitKey</a>(100);</div><div class="line"></div><div class="line">            <span class="keywordflow">if</span>( code &gt; 0 )</div><div class="line">                <span class="keywordflow">break</span>;</div><div class="line">        }</div><div class="line">        <span class="keywordflow">if</span>( code == 27 || code == <span class="charliteral">&#39;q&#39;</span> || code == <span class="charliteral">&#39;Q&#39;</span> )</div><div class="line">            <span class="keywordflow">break</span>;</div><div class="line">    }</div><div class="line"></div><div class="line">    <span class="keywordflow">return</span> 0;</div><div class="line">}</div></div><!-- fragment --> </div><!-- contents -->
<!-- HTML footer for doxygen 1.8.6-->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated on Fri Apr 2 2021 11:36:29 for OpenCV by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="../../doxygen.png" alt="doxygen"/>
</a> 1.8.13
</small></address>
<script type="text/javascript">
//<![CDATA[
addTutorialsButtons();
//]]>
</script>
</body>
</html>
